// model
#include "model.h"
#include <stdio.h>
#include "glotech_headers.h"

#ifndef ERROR_MARGIN
	#define ERROR_MARGIN 7
#endif

struct node{
	float pos_x;
	float pos_y;
	float pos_z;
	float theta_x;
	float theta_y;
	float theta_z;
	float radius_x;
	float radius_y;
	float radius_z;
	struct node * next;
};

typedef struct node NODE;

struct queue{
	int cnt;
	NODE * head;
	NODE * tail;
};

typedef struct queue QUEUE;



int difference(float theoretical, float measured){
	if(((theoretical-measured)/theoretical) * 100 > ERROR_MARGIN)
		return FAILURE;
	return SUCCESS;
}

void calculate_radius_node(NODE * n){
	n->radius_x = (n->pos_x)*(1-ERROR_MARGIN);
	n->radius_y = (n->pos_y)*(1-ERROR_MARGIN);
	n->radius_z = (n->pos_z)*(1-ERROR_MARGIN);
}

void calculate_radius(QUEUE * q){
	NODE * next =  q->head->next;

	while(next != 0){
		calculate_radius_node(next);
		next = next->next;
	}
	calculate_radius_node(q->tail);
}

MODEL * init_model(void){
	MODEL * m;
	QUEUE * q_expert;
	QUEUE * q_tech;

	m = (MODEL *) malloc(sizeof(MODEL));
	if(m == NULL)
		return NULL;
	q_expert = init_queue();
	q_tech = init_queue();
	if((q_expert == NULL) || (q_tech == NULL)){
		free(m);
		free(q_tech);
		free(q_expert);
		return NULL;
	}
	m->q_expert = q_expert;
	m->q_technician = q_tech;
	return m;
}

void destroy_model(MODEL * m){
	destroy_queue(m->q_expert);
	destroy_queue(m->q_technician);
	free(m);
}

QUEUE * init_queue(void){
	QUEUE * new_queue;
	new_queue = (QUEUE *) malloc(sizeof(QUEUE));
	if(new_queue == NULL)
		return NULL;
	new_queue->cnt = 0;
	new_queue->head = 0;
	new_queue->tail = 0;
	return new_queue;
}

int push(QUEUE * q, float pos_x, float pos_y, float pos_z, float theta_x, float theta_y, float theta_z){
	NODE * new_node;
	new_node = (NODE *) malloc(sizeof(NODE));
	if(new_node == NULL){
		return FAILURE;
	}

	new_node->next = 0;
	new_node->pos_x = pos_x;
	new_node->pos_y = pos_y;
	new_node->pos_z = pos_z;
	new_node->theta_x = theta_x;
	new_node->theta_y = theta_y;
	new_node->theta_z = theta_z;
	new_node->radius_x = 0;
	new_node->radius_y = 0;
	new_node->radius_z = 0;

	if(q->cnt == 0){
		q->head = new_node;
		q->tail = new_node;
	} else {
		q->tail->next = new_node;
		q->tail = new_node;
	}
	q->cnt++;
	return SUCCESS;
}

NODE * pop(QUEUE * q){
	NODE * poped;
	if(q->cnt == 0)
		return NULL;
	poped = q->head;
	q->head = q->head->next;
	q->cnt--;
	return poped;
}

int is_queue_empty(QUEUE * q){
	if(q->cnt == 0)
		return YES;
	return NO;
}

void free_node(NODE * n){
	free(n);
}

void destroy_queue(QUEUE * q){
	NODE * n;
	while(q->cnt != 0){
		n = pop(q);
		free_node(n);
	}
	free(q);
}

void print_node(NODE * n){
	printf("Node: 0x%x\n", n);
	printf("Pos X: %f\tTheta X: %f\n", n->pos_x, n->theta_x);
	printf("Pos Y: %f\tTheta Y: %f\n", n->pos_y, n->theta_y);
	printf("Pos Z: %f\tTheta Z: %f\n\n", n->pos_z, n->theta_z);
}

void print_queue(QUEUE * q){
	NODE * next =  q->head->next;
	int cnt = 0;

	printf("Queue: 0x%x\n", q);
	printf("Node Count: %i\n", q->cnt);
	printf("Head: 0x%x\n", q->head);
	printf("Tail: 0x%x\n\n", q->tail);

	print_node(q->head);
	while(next != 0){
		print_node(next);
		next = next->next;
		cnt++;
	}
}